#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#define Move1 1000
#define Move2 2000
#define Move3 10
#define Move4 20
#define Move5 30
#define Move6 40
#define Move7 50
#define Move8 60
#define Move9 70
#define Move10 80
#define Move11 90
#define Move12 100
#define Move13 110
#define Move14 120
#define Move15 130
#define Move16 123
#define Move17 124
#define Move18 234
#define Move19 789
#define Move20 98
#define Move21 23

#define FlipperToFloor 0
#define FlipperScore -100
#define FlipperUp 100
#define FlipperHoldHeavy 25
#define FlipperHoldLight 15
#define FlipperFast 127
#define FlipperSlow 60

#define ArmsFloor 0
#define ArmsTrough 1000
#define ArmsHigh 2000
#define ArmsFast 127
#define ArmsSlow 60

// remember to add back in the funstions we worked on at home - gyro turning and lcd button press

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void Turn90Degrees(bool bTurnRight)
{
        //will read gyro to determine 90 degrees
        // the 2 wheel motors, one will be postitive one will be negative depending on which way we turn
}

void Move(bool bForward, int iDistance)
{
        // Reset encoders
        // true forwards, false backwards
        // iDistance tells the encoder how far to go
}

void FlipperHold(bool bHeavy)
{
  //Heavy load(true) Light load(false)
}

void Flipper(bool bUp, int iPosition, int iMotorPower)
{
  //Flip the flipper up(true) or down(false)
}

void Arms(bool bUp, int iPosition, int iMotorPower)
{
	//Move arms to trough, floor, or high
}

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

        // All activities that occur before the competition starts
        // Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
 	 			//Start Robot facing forwards
				//Press button to activate robot
				while(true)
				{
					//to do- check if button has been pressed if so break out of loop
					break;
				}
				//Flipper folds down
				Flipper(false, FlipperToFloor, FlipperFast);
				//Place 5 preloads on to flipper
				while(true)
				{
					//to do- check if button has been pressed if so break out of loop
					break;
				}
				//Press button to send the robot moving while arms go up---Raise arms to trough level
				Flipper(true, FlipperToFloor, FlipperHoldHeavy);
				Arms(true, ArmsTrough, ArmsFast);
				//Move forward - turn right - move forward - turn left - to trough
				Move(true, Move1);
				Turn90Degrees(true);
				Move(true, Move2);
				Turn90Degrees(false);
				Move(true, Move3);
				//Flipper moves down to Empty 5 bags - 25 points (total 25)
				Flipper(false, FlipperScore, FlipperFast);
				//Flipper moves back to pickup position
        Flipper(true, FlipperToFloor, FlipperFast);
        //Back up
        Move(false, Move4);
				//Arms move to floor
        Arms(false, ArmsFloor, ArmsFast);
				//move forward - pick up 2 sacks (one is bonus sack)
        Move(true, Move5);
      	FlipperHold(false);
				//Robot move backwards
        Move(false, Move6);
				//Arms move up to trough
        Arms(true, ArmsTrough, ArmsFast);
				//Robot moves forward
        Move(true, Move7);
				//Flipper moves down to Empty 2 bags - 15 points (total 40)
        Flipper(false, FlipperScore, FlipperFast);
				//Flipper moves back to pickup position
        Flipper(true, FlipperToFloor, FlipperFast);
				//Back up
        Move(false, Move8);
				//Turn Left
        Turn90Degrees(false);
				//Arms go to ground
        Arms(false, ArmsFloor, ArmsFast);
				//Move forward to pickup 5 bags
        Move(true, Move9);
        FlipperHold(true);
				//Arm raises up
        Arms(true, ArmsTrough, ArmsFast);
				//Robot turns right
        Turn90Degrees(true);
				//Robot moves forward
        Move(true, Move10);
				//Flipper moves down to Empty 5 bags - 25 points (total 65)
        Flipper(false, FlipperScore, FlipperFast);
        Flipper(true, FlipperToFloor, FlipperFast);
				//Back up
        Move(false, Move11);
				//Turn Left
        Turn90Degrees(false);
				//Arms go to ground
        Arms(false, ArmsFloor, ArmsFast);
				//Move forward to bonus bag
        Move(true, Move12);
       	FlipperHold(false);
				//Arm raises up
       	Arms(true, ArmsTrough, ArmsFast);
				//Robot turns right
        Turn90Degrees(true);
				//Robot moves forward
        Move(true, Move13);
				//Flipper moves down to Empty 1 bags - 15 points (total 80)
        Flipper(false, FlipperScore, FlipperFast);
        Flipper(true, FlipperToFloor, FlipperFast);
				//Back up
        Move(false, Move14);
				//Turn Left
        Turn90Degrees(false);
				//Arms go to ground
				//Move forward to pickup 5 bags
        Move(true, Move15);
        FlipperHold(true);
				//Arm raises up
				//Robot turns right
        Turn90Degrees(true);
				//Robot moves forward
        Move(true, Move16);
				//Flipper moves down to Empty 5 bags - 25 points (total 105)
        Flipper(false, FlipperScore, FlipperFast);
        Flipper(true, FlipperToFloor, FlipperFast);
				//Back up
        Move(false, Move17);
				//Arms move to floor
        Arms(false, ArmsFloor, ArmsFast);
				//move forward - pick up 2 sacks (one is bonus sack)
        Move(true, Move18);
        FlipperHold(false);
				//Robot move backwards
        Move(false, Move19);
				//Arms move up to trough
        Arms(true, ArmsTrough, ArmsFast);
				//Robot moves forward
        Move(true, Move20);
				//Flipper moves down to Empty 2 bags - 15 points (total 120)
        Flipper(false, FlipperScore, FlipperFast);
				//Flipper moves back to pickup position
        Flipper(true, FlipperToFloor, FlipperFast);
				//Robot moves backwards towards a starting tile
        Move(false, Move21);
				//Manually move robot to ensure that it is completely touching starting tile


        AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
        // User control code here, inside the loop

        while (true)
        {
          // This is the main execution loop for the user control program. Each time through the loop
          // your program should update motor + servo values based on feedback from the joysticks.

          // .....................................................................................
          // Insert user code here. This is where you use the joystick values to update your motors, etc.
          // .....................................................................................

          UserControlCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
        }
}
